#include "astar_planner.h"

int main()
{
    // 1. 解析YAML配置（含膨胀核大小）
    std::string yaml_path = "../open_space_map.yaml";  // 注意路径：若在build目录运行，需指向上级map.yaml
    MapParams params = parseYaml(yaml_path);

    // 打印地图参数（含膨胀配置）
    std::cout << "===================== 地图参数 =====================" << std::endl;
    std::cout << "分辨率: " << params.resolution << " 米/像素" << std::endl;
    std::cout << "原点: (" << params.origin.x << ", " << params.origin.y << ")" << std::endl;
    std::cout << "障碍物阈值: " << params.occupied_thresh << std::endl;
    std::cout << "障碍膨胀核大小: " << params.dilate_kernel_size << "x" << params.dilate_kernel_size << std::endl;
    std::cout << "PGM路径: " << params.image_path << std::endl;
    std::cout << "====================================================" << std::endl;

    // 2. 加载原始二值地图
    cv::Mat grid_map = loadMap(params);
    std::cout << "原始地图尺寸: " << grid_map.cols << "x" << grid_map.rows << " 像素" << std::endl;

    // 3. 执行障碍膨胀（核心步骤：修改地图，扩展障碍物）
    dilateObstacles(grid_map, params.dilate_kernel_size);
    std::cout << "膨胀后地图尺寸: " << grid_map.cols << "x" << grid_map.rows << " 像素" << std::endl;

    // 4. 设置起点/终点（世界坐标，单位：米）
    cv::Point2d start_world(-5.0, 35.0);  // 起点（根据实际地图调整）
    cv::Point2d goal_world(0.0, 0.0);     // 终点（根据实际地图调整）

    // 转换为栅格坐标
    cv::Point start_grid = worldToGrid(start_world, params);
    cv::Point goal_grid = worldToGrid(goal_world, params);
    std::cout << "起点（栅格）: (" << start_grid.x << ", " << start_grid.y << ")" << std::endl;
    std::cout << "终点（栅格）: (" << goal_grid.x << ", " << goal_grid.y << ")" << std::endl;

    // 5. 执行A*路径规划（使用膨胀后的地图）
    std::vector<cv::Point> path = aStarSearch(grid_map, start_grid, goal_grid);
    if (path.empty())
    {
        std::cerr << "程序终止：未找到路径" << std::endl;
        return 1;
    }
    std::cout << "找到路径，共 " << path.size() << " 个节点" << std::endl;

    // 6. 可视化结果（障碍：黑色，自由：白色，起点：绿色，终点：红色，路径：蓝色）
    cv::Mat map_vis;
    cv::cvtColor(grid_map * 255, map_vis, cv::COLOR_GRAY2BGR);  // 二值图转彩色图

    // 绘制起点和终点
    cv::circle(map_vis, start_grid, 5, cv::Scalar(0, 255, 0), -1);  // 起点：绿色实心圆
    cv::circle(map_vis, goal_grid, 5, cv::Scalar(0, 0, 255), -1);   // 终点：红色实心圆

    // 绘制路径
    for (size_t i = 1; i < path.size(); ++i)
    {
        cv::line(map_vis, path[i-1], path[i], cv::Scalar(255, 0, 0), 2);  // 路径：蓝色实线
    }

    // 显示结果
    cv::namedWindow("A* Path Planning (Obstacle Dilated)", cv::WINDOW_NORMAL);
    cv::imshow("A* Path Planning (Obstacle Dilated)", map_vis);
    std::cout << "按下任意键关闭窗口..." << std::endl;
    cv::waitKey(0);  // 等待按键关闭窗口

    return 0;
}